#include "container.h"
#include "factories.h"
#include "textrenderer.h"


// Picker implementation

PickContainer::PickContainer()
: selected(false)
{
    SetVec3f(position, 0, 0, 0);
    SetVec3f(scaling, 1, 1, 1);
    
    SetVec3f(boundingBox.min, 0, 0, 0);
    SetVec3f(boundingBox.max, 0, 0, 0);
}

void PickContainer::worldToContainerMat4f(Mat4f out) const
{
    Mat4f invScale, invTranslate;
    InverseScaleMat4f(invScale, scaling);
    InverseTranslateMat4f(invTranslate, position);
    MultiplyMat4f(out, invScale, invTranslate);
}

void PickContainer::worldToContainerDirectionMat4f(Mat4f out) const
{
    InverseScaleMat4f(out, scaling);
}

void PickContainer::containerToWorldMat4f(Mat4f out) const
{
    Mat4f translate, scale;
    TranslateMat4f(translate, position);
    ScaleMat4f(scale, scaling);
    MultiplyMat4f(out, translate, scale);
}

void PickContainer::containerToWorldDirectionMat4f(Mat4f out) const
{
    ScaleMat4f(out, scaling);
}

void PickContainer::addElement(Element* element)
{
    elements.append(element);
    boundingBox.merge(element->boundingBox);
}

QVector<Element*> PickContainer::pickElementsWithRay(const Ray& ray) const
{
    QVector<Element*> pickedElements;
    Ray containerRelativeRay = getContainerRelativeRay(ray);
    for (int e = 0; e < elements.size(); e++) {
        float distance;
        Vec3f intersection;
        Element* element = elements.at(e);
        if (containerRelativeRay.intersect(element->boundingBox, distance, intersection))
            pickedElements.append(element);
    }
    return pickedElements;
}

bool PickContainer::hit(const Ray& ray) const
{
	if (ray.direction[2] == 0)
		return false;
	
	float distance;
	Vec3f intersection;
	return getContainerRelativeRay(ray).intersect(labeledBoundingBox, distance, intersection);
}

void PickContainer::layoutWithCamera(Camera& camera)
{
  if ( labeled 
    && name.length() > 0
    && boundingBox.height() > 0  // if we won't draw anything else, don't reserve place for a label
     ) 
  {
    TextRenderer& renderer = TextRenderer::instance();
    renderer.setInputAlignment(TextRenderer::Left, TextRenderer::Top);
    renderer.setOutputAlignment(TextRenderer::Left, TextRenderer::Bottom);
    labelBoundingBox = renderer.getWorldBoundingBoxWithCamera(name.toStdString().c_str(), camera, scaling[1]);
    labeledBoundingBox = boundingBox;
    labeledBoundingBox.min[1] -= labelBoundingBox.height();
    labeledBoundingBox.max[1] += labelBoundingBox.height() / 2.0f;
  }
  else
  {
    labeledBoundingBox = boundingBox;
  }
}

Ray PickContainer::getContainerRelativeRay(const Ray& ray) const
{
    Mat4f worldToContainer, worldToContainerDirection;
    worldToContainerMat4f(worldToContainer);
    worldToContainerDirectionMat4f(worldToContainerDirection);
    
    Ray containerRelativeRay(ray);
    MultiplyMat4fVec3f(containerRelativeRay.origin, worldToContainer, ray.origin);
    MultiplyMat4fVec3f(containerRelativeRay.direction, worldToContainerDirection, ray.direction);
    
    return containerRelativeRay;
}

QList<Element*> PickContainer::getSelection() const
{
	return selectedElements.values();
}


// Container implementation

Container::Container()
: compiledElements(NULL), modified(true), root(NULL), elementsVerticesSize(0), elementsIndicesSize(0)
{
	SetVec3f(boundingBox.min, 0, 0, 0);
	SetVec3f(boundingBox.max, 0, 0, 0);
}

Container::~Container()
{
    if (compiledElements)
        delete compiledElements;
    
    if (root)
        delete root;
    
    for (unsigned element = 0; element < (unsigned)elements.size(); element++)
        delete elements.at(element);
}

void Container::addElement(Element* element)
{
    // Mutation is not allowed after the compilation of the container
    if (compiledElements)
        return;
    
    elements.append(element);
    boundingBox.merge(element->boundingBox);
    elementsVerticesSize += element->getVerticesSize();
    elementsIndicesSize += element->getIndicesIndex();
}

void Container::compile()
{
    if (elements.size() == 0 || root)
        return;
    
    // Partition the space with its elements
    root = new PartitionTreeNode(boundingBox.min[0], boundingBox.max[0]);
    root->split(elements);
    
    // Order the elements in the vertex array based on the left to right order of the leaves in the partition tree
    VertexArray* vertexArray = new VertexArray(elementsVerticesSize, elementsIndicesSize);
    compiledElements = new VertexBufferObject(vertexArray);
    unsigned verticesIndex = 0, indicesIndex = 0;
    QVector<PartitionTreeNode*> leaves = root->getLeaves();
    QSet<Element*> addedElements;
    for (unsigned l = 0; l < (unsigned)leaves.size(); l++) {
        PartitionTreeNode* leaf = leaves.at(l);
        Element** currentElement = leaf->elements;
        unsigned count = leaf->elementsSize;
        while (count--) {
            if (!addedElements.contains(*currentElement)) {
                addedElements.insert(*currentElement);
                (*currentElement)->assign(vertexArray, verticesIndex, indicesIndex);
            }
            currentElement++;
        }
    }
    
    // The nodes have to be updated with the generated vertex array index data
    root->updateIndices();
}

void Container::reassignColors()
{
	if (elements.size() == 0 || !compiledElements)
		return;
	
	QVectorIterator<Element*> elementsIter(elements);
	while (elementsIter.hasNext()) {
		Element* element = elementsIter.next();
		Color4b color;
		element->getFinalColor(color);
		
		PackedVecColor* vertices = getElementVertices(element);
		unsigned count = 4;
		while (count--) {
			SetColor4b(vertices->c, color);
			vertices++;
		}
	}
	
	modified = true;
}

void Container::drawVBOWithCamera(Camera& camera)
{
    if (!compiledElements)
        return;
	
	if (modified) {
		modified = false;
		compiledElements->send(false);
	}

    // Intersect the X-axis with the left and right camera planes
    Vec4f leftComp, rightComp;
    Vec3f positionScale = {1, 1, 1};
    camera.getLeftPlaneComponents(leftComp, positionScale, 0, 1);
    camera.getRightPlaneComponents(rightComp, positionScale, 0, 1);
    float startPos = floorf(-leftComp[3] / (leftComp[0] * scaling[0]) - position[0]);
    float endPos = ceilf(-rightComp[3] / (rightComp[0] * scaling[0]) - position[0]);
    
    // Find the leaf nodes corresponding with the start and end positions
    unsigned indexStart = root->getLeaf(startPos)->indexStart;
    unsigned indexEnd = root->getLeaf(endPos)->indexEnd;
    unsigned length = indexEnd - indexStart + 4;
    
    // Draw the range of the vertex buffer object
    glPushMatrix();
    glTranslatef(position[0], position[1], position[2]);
    glScalef(scaling[0], scaling[1], scaling[2]);
    compiledElements->oneShotDraw(GL_QUADS, indexStart, length);
    glPopMatrix();
}

void Container::drawVAWithCamera(Camera& camera) const
{
    if (!compiledElements)
        return;
    
    glPushMatrix();
    
    // Setup the transformation for the container
    glTranslatef(position[0], position[1], position[2]);
    glScalef(scaling[0], scaling[1], scaling[2]);
    
    // Setup the color since the color array is not used
    glColor4ubv(ColorFactory::instance().getSelectionColor());
    
    // Create an index array for a closed line strip
    GLubyte lineStripIndices[] = {3, 0, 1, 2, 3, 1};
    GLsizei stride = sizeof(PackedVecColor);
    
    QSetIterator<Element*> selectionIter(selectedElements);
    while (selectionIter.hasNext()) {
        Element* element = selectionIter.next();
        glVertexPointer(3, GL_FLOAT, stride, (GLfloat *)getElementVertices(element));
        glDrawElements(GL_LINE_STRIP, sizeof(lineStripIndices), GL_UNSIGNED_BYTE, lineStripIndices);
    }
    
    glPopMatrix();
}

void Container::drawBackgroundWithCamera(Camera& camera)
{
    if (!compiledElements)
        return;
    
    glPushMatrix();
    
    // Setup the transformation for the container
    glTranslatef(position[0], position[1], position[2]);
    glScalef(scaling[0], scaling[1], scaling[2]);
    
	Color4b color;
	BoundingBox bb = labeledBoundingBox;
//	bb.inset(0, -0.5, 0);
    VertexArray* containerRectangle = ObjectFactory::instance().createRectangle(bb.min, bb.max, color);

    // Create an index array for a closed line strip
    GLubyte lineStripIndices[] = {0, 1, 2, 3, 0};
    GLsizei stride = sizeof(PackedVecColor);
    
    // Setup the color for the background
    glColor4ubv(selected ? ColorFactory::instance().getContainerSelectedColor() : ColorFactory::instance().getContainerColor());
	glVertexPointer(3, GL_FLOAT, stride, (GLfloat *)containerRectangle->getVertices());
	glDrawArrays(GL_QUADS, 0, containerRectangle->getVerticesSize());

    // Setup the color for the border
    glColor4ubv(ColorFactory::instance().getContainerBorderColor());
	glDrawElements(GL_LINE_STRIP, sizeof(lineStripIndices), GL_UNSIGNED_BYTE, lineStripIndices);
    
    glPopMatrix();
	
	delete containerRectangle;
}

void Container::drawTextWithCamera(Camera& camera) const
{
	if (!compiledElements)
		return;
	
    // Intersect the X-axis with the left and right camera planes
    Vec4f leftComp, rightComp;
    Vec3f positionScale = {1, 1, 1};
    camera.getLeftPlaneComponents(leftComp, positionScale, 0, 1);
    camera.getRightPlaneComponents(rightComp, positionScale, 0, 1);
    float leftClip = -leftComp[3] / (leftComp[0] * scaling[0]) - position[0];
    float rightClip = -rightComp[3] / (rightComp[0] * scaling[0]) - position[0];
	float startPos = floorf(leftClip);
	float endPos = ceilf(rightClip);
    
    // Find the leaf nodes corresponding with the start and end positions
	bool done = false;
    QVector<PartitionTreeNode*> drawnLeaves = root->getLeaf(startPos)->getLeaves(root->getLeaf(endPos), NULL, done);
    
    // Set the correct transformation for the text
    glPushMatrix();
    glTranslatef(position[0], position[1], position[2]);
    glScalef(scaling[0], scaling[1], scaling[2]);
	
	// Set the correct alignment for the text rendering
	TextRenderer& renderer = TextRenderer::instance();
	renderer.setInputAlignment(TextRenderer::Left, TextRenderer::Bottom);
	if (camera.getYaw() >= 180.0f || camera.getYaw() == 0.0f)
		renderer.setOutputAlignment(TextRenderer::Left, TextRenderer::Bottom);
	else
		renderer.setOutputAlignment(TextRenderer::Right, TextRenderer::Bottom);
	
	// Process all elements of the drawn leaves
	QSet<Element*> processedElements;
	QVectorIterator<PartitionTreeNode*> drawnLeavesIter(drawnLeaves);
	while (drawnLeavesIter.hasNext()) {
		PartitionTreeNode* drawnLeaf = drawnLeavesIter.next();
		Element** element = drawnLeaf->elements;
		unsigned count = drawnLeaf->elementsSize;
		while (count--) {
			if (!processedElements.contains(*element)) {
				processedElements.insert(*element);
				
				// Get the text position;
				BoundingBox bb = (*element)->boundingBox;
				float left = bb.min[0];
				float right = bb.max[0];
				
				if (right > leftClip && left < rightClip) {
					
					// Get the clipped text position
					if (left < leftClip)
						left = leftClip;
					if (right > rightClip)
						right = rightClip;
					
					float width = right - left;
					if (width > 0.0f) {
						glPushMatrix();
						
						// Move to the bottom left of the element
						glTranslatef(left, bb.min[1], bb.min[2]);
						
						// Revert the scaling of the text, since all scaling is handled by the text renderer
						glScalef(1.0f/scaling[0], 1.0f/scaling[1], 1.0f/scaling[2]);
						
						// Draw the text
						renderer.drawWithCamera((*element)->getDescription().toStdString().c_str(), camera, bb.height() * scaling[1], width * scaling[0]);
						
						glPopMatrix();
					}
				}
			}
			element++;
		}
	}
	
	// Draw the name of the container if requested
	if (labeled) {
		if (camera.getYaw() >= 180.0f || camera.getYaw() == 0.0f) {
			renderer.setInputAlignment(TextRenderer::Left, TextRenderer::Top);
			renderer.setOutputAlignment(TextRenderer::Left, TextRenderer::Bottom);
		}
		else {
			renderer.setInputAlignment(TextRenderer::Right, TextRenderer::Top);
			renderer.setOutputAlignment(TextRenderer::Right, TextRenderer::Bottom);
		}
		float labelLeft = boundingBox.min[0];
		float labelRight = boundingBox.max[0];
		if (labelRight > leftClip && labelLeft < rightClip) {
			
			// Get the clipped text position
			if (labelLeft < leftClip)
				labelLeft = leftClip;
			if (labelRight > rightClip)
				labelRight = rightClip;
			
			float labelWidth = labelRight - labelLeft;
			if (labelWidth > 0.0f) {
				glPushMatrix();
				
				// Move to the bottom left of the element
				if (camera.getYaw() >= 180.0f || camera.getYaw() == 0.0f)
					glTranslatef(labelLeft, boundingBox.min[1], boundingBox.min[2]);
				else
					glTranslatef(labelRight, boundingBox.min[1], boundingBox.min[2]);
				
				// Revert the scaling of the text, since all scaling is handled by the text renderer
				glScalef(1.0f/scaling[0], 1.0f/scaling[1], 1.0f/scaling[2]);
				
				// Draw the text
				renderer.drawWithCamera(name.toStdString().c_str(), camera, labelBoundingBox.height());
				
				glPopMatrix();
			}
		}
	}
	
    glPopMatrix();
}

QVector<Element*> Container::pickElementsWithRay(const Ray& ray) const
{
    QVector<Element*> pickedElements;
    
	if (elements.size() == 0)
		return pickedElements;
	
    // Avoid explosions with rays parallel with the horizontal axis
    if (ray.direction[2] == 0)
        return pickedElements;
    
    Ray containerRelativeRay = getContainerRelativeRay(ray);
    
    // Calculate the horizontal position of the ray when Z is zero
    Vec3f intersection;
    containerRelativeRay.getPoint(intersection, 0);
    
    // Pick the elements in the leaf with an adjusted ray origin
    PartitionTreeNode *leaf = root->getLeaf(intersection[0]);
    Element** currentElement = leaf->elements;
    unsigned count = leaf->elementsSize;
    while (count--) {
        float distance;
        Vec3f intersection;
        if (containerRelativeRay.intersect((*currentElement)->boundingBox, distance, intersection))
            pickedElements.append(*currentElement);
        currentElement++;
    }
    
    return pickedElements;
}

bool happensBefore(const Element* left, const Element* right)
{
	return left->boundingBox.min[0] < right->boundingBox.max[0];
}

QVector<Element*> Container::leftClippedElements(Camera& camera, unsigned maxSize, bool& hitsFirst) const
{
	QVector<Element*> leftElements;

	if (!compiledElements)
		return leftElements;
	
    // Intersect the X-axis with the left camera plane
    Vec4f leftComp;
    Vec3f positionScale = {1, 1, 1};
    camera.getLeftPlaneComponents(leftComp, positionScale, 0, 1);
    float leftClip = -leftComp[3] / (leftComp[0] * scaling[0]) - position[0];
    
    // Find the leaf nodes corresponding with the start and end positions
	bool done = false;
    QVector<PartitionTreeNode*> drawnLeaves = root->getLeaf(boundingBox.min[0])->getLeaves(root->getLeaf(ceilf(leftClip)), NULL, done);
	
	// Process all elements of the drawn leaves
	QSet<Element*> processedElements;
	QVectorIterator<PartitionTreeNode*> drawnLeavesIter(drawnLeaves);
	while (drawnLeavesIter.hasNext()) {
		PartitionTreeNode* drawnLeaf = drawnLeavesIter.next();
		Element** element = drawnLeaf->elements;
		unsigned count = drawnLeaf->elementsSize;
		while (count--) {
			if (!processedElements.contains(*element)) {
				processedElements.insert(*element);
				
				// Get the position;
				BoundingBox bb = (*element)->boundingBox;
				float left = bb.min[0];
				float right = bb.max[0];
				
				if (left < leftClip) {
					if (right > leftClip)
						hitsFirst = true;
					
					leftElements.append(*element);
				}
			}
			element++;
		}
	}
	
	qSort(leftElements.begin(), leftElements.end(), happensBefore);
	if (leftElements.size() > (int)maxSize)
		leftElements.remove(0, leftElements.size() - maxSize);
	
	return leftElements;
}

QVector<Element*> Container::rightClippedElements(Camera& camera, unsigned maxSize, bool& hitsFirst) const
{
	QVector<Element*> rightElements;

	if (!compiledElements)
		return rightElements;
	
    // Intersect the X-axis with the left camera plane
    Vec4f rightComp;
    Vec3f positionScale = {1, 1, 1};
    camera.getRightPlaneComponents(rightComp, positionScale, 0, 1);
    float rightClip = -rightComp[3] / (rightComp[0] * scaling[0]) - position[0];
    
    // Find the leaf nodes corresponding with the start and end positions
	bool done = false;
    QVector<PartitionTreeNode*> drawnLeaves = root->getLeaf(floorf(rightClip))->getLeaves(root->getLeaf(boundingBox.max[0]), NULL, done);
	
	// Process all elements of the drawn leaves
	QSet<Element*> processedElements;
	QVectorIterator<PartitionTreeNode*> drawnLeavesIter(drawnLeaves);
	while (drawnLeavesIter.hasNext()) {
		PartitionTreeNode* drawnLeaf = drawnLeavesIter.next();
		Element** element = drawnLeaf->elements;
		unsigned count = drawnLeaf->elementsSize;
		while (count--) {
			if (!processedElements.contains(*element)) {
				processedElements.insert(*element);
				
				// Get the position;
				BoundingBox bb = (*element)->boundingBox;
				float left = bb.min[0];
				float right = bb.max[0];
				
				if (right > rightClip) {
					if (left < rightClip)
						hitsFirst = true;
					
					rightElements.append(*element);
				}
			}
			element++;
		}
	}
	
	qSort(rightElements.begin(), rightElements.end(), happensBefore);
	if (rightElements.size() > (int)maxSize)
		rightElements.remove(maxSize, rightElements.size() - maxSize);
	
	return rightElements;
}

PackedVecColor* Container::getElementVertices(Element* element) const
{
    return compiledElements->getVertexArray()->getVertices() + element->getVerticesIndex();
}

GLuint* Container::getElementIndices(Element* element) const
{
    return compiledElements->getVertexArray()->getIndices() + element->getIndicesIndex();
}


// PartitionTreeNode implementation

unsigned PartitionTreeNode::minimalElementCountForSplit = 128;

PartitionTreeNode::PartitionTreeNode(float start, float end)
: start(start), end(end), parent(NULL), leftChildNode(NULL), rightChildNode(NULL), indexStart(0), indexEnd(0), elements(NULL), elementsSize(0)
{
}

PartitionTreeNode::~PartitionTreeNode()
{
    if (elements)
        delete [] elements;
    
    if (leftChildNode)
        delete leftChildNode;
    if (rightChildNode)
        delete rightChildNode;
}

void PartitionTreeNode::split(const QVector<Element*>& elements)
{
    unsigned elementsSize = elements.size();
    
    // Only split when necessary, else create a leaf containing the elements
    if (elementsSize < minimalElementCountForSplit) {
        this->elementsSize = elementsSize;
        this->elements = new Element*[elementsSize];
        for (unsigned element = 0; element < elementsSize; element++)
            this->elements[element] = elements.at(element);
        return;
    }
    
    // Divide the elements over the splits
    float center = getSplitPosition();
    QVector<Element*> left, right;
    for (unsigned element = 0; element < elementsSize; element++) {
        Element* e = elements.at(element);
        if (e->boundingBox.min[0] <= center)
            left.append(e);
        if (e->boundingBox.max[0] >= center)
            right.append(e);
    }
    
    qDebug() << "center: " << center << " start: " << start << " end: " << end;
    qDebug() << "total: " << elementsSize << " left: " << left.size() << " right: " << right.size();
    
    // Continue the splitting process recursively
    if (left.size() > 0) {
        setLeftChildNode(new PartitionTreeNode(start, center));
        leftChildNode->split(left);
    }
    
    if (right.size() > 0) {
        setRightChildNode(new PartitionTreeNode(center, end));
        rightChildNode->split(right);
    }
}

void PartitionTreeNode::updateIndices(unsigned prevIndexEnd)
{
    if (isLeaf()) {
        if (!elementsSize)
            return;
        
        // Derive the range of the node based on its elements
        Element** currentElement = elements;
        indexStart = (*currentElement)->getIndex();
        indexEnd = prevIndexEnd;

        currentElement++;
        unsigned count = elementsSize - 1;
        while (count--) {
            unsigned index = (*currentElement)->getIndex();
            if (index < indexStart)
                indexStart = index;
            if (index > indexEnd)
                indexEnd = index;
            currentElement++;
        }
    }
    else if (leftChildNode && rightChildNode) {
        leftChildNode->updateIndices(prevIndexEnd);
        rightChildNode->updateIndices(leftChildNode->indexEnd);
        indexStart = leftChildNode->indexStart;
        indexEnd = rightChildNode->indexEnd;
    }
    else if (leftChildNode) {
        leftChildNode->updateIndices(prevIndexEnd);
        indexStart = leftChildNode->indexStart;
        indexEnd = leftChildNode->indexEnd;
    }
    else if (rightChildNode) {
        rightChildNode->updateIndices(prevIndexEnd);
        indexStart = rightChildNode->indexStart;
        indexEnd = rightChildNode->indexEnd;
    }
}

PartitionTreeNode* PartitionTreeNode::getLeaf(float position)
{
    if (isLeaf())
        return this;
    
    if (position <= getSplitPosition())
        return (leftChildNode ? leftChildNode->getLeaf(position) : this);
    else
        return (rightChildNode ? rightChildNode->getLeaf(position) : this);
}

QVector<PartitionTreeNode*> PartitionTreeNode::getLeaves()
{
    QVector<PartitionTreeNode*> leaves;
    if (isLeaf()) {
        leaves.append(this);
        return leaves;
    }
    
    if (leftChildNode)
        leaves += leftChildNode->getLeaves();
    
    if (rightChildNode)
        leaves += rightChildNode->getLeaves();
    
    return leaves;
}

QVector<PartitionTreeNode*> PartitionTreeNode::getLeaves(PartitionTreeNode* includedEndNode, PartitionTreeNode* previous, bool& done)
{
	QVector<PartitionTreeNode*> leaves;
	
	if (isLeaf()) {
		leaves.append(this);
		done = (this == includedEndNode);
		
		if (!done && parent && !previous) {
			// We need to go up to keep going to the right of the tree
			leaves += parent->getLeaves(includedEndNode, this, done);
		}
		
		return leaves;
	}
	
	if (!done && leftChildNode && leftChildNode != previous && rightChildNode != previous) {
		leaves += leftChildNode->getLeaves(includedEndNode, this, done);
	}
	
	if (!done && rightChildNode && rightChildNode != previous) {
		leaves += rightChildNode->getLeaves(includedEndNode, this, done);
	}
	
	if (!done && parent) {
		// Still not found the end leaf, so keep going up
		leaves += parent->getLeaves(includedEndNode, this, done);
	}
	
	return leaves;
}
